# PA_TLT.py: 计算倾斜角度和位置的函数
from math import sin,cos,pi,atan,tan

def cal_tlt(t,xs,xf,h,r1,r2,r3,r4):
    Hc=100
    Tm=0.5
    z=0
    d=atan(z/Hc)
    H0=h
    a=0
    zz=0
    #方向判断
    if r1==-1 and r2==-1 and r3==1 and r4==1:   #转左
        zz=-15
    elif r1==1 and r2==1 and r3==-1 and r4==-1:   #转左
        zz=15
    elif r1==1 and r2==1 and r3==1 and r4==1:   #转左
        zz=0

    if t<=Tm:
        c=cos(d*cos(pi*t/Tm));
        b=(0.5-0.5*cos(2*pi*t/Tm))
        x0=0
        y0=(H0*b)/c
        z0=a+zz*cos(pi*t/Tm)
        x1=0
        y1=(-H0/40*b)/c
        z1=a+zz*cos(pi*t/Tm)
        x2=0
        y2=y1
        z2=a+zz*cos(pi*t/Tm+pi)
        x3=0
        y3=y0
        z3=a+zz*cos(pi*t/Tm+pi)
        return x0,x1,x3,x2,y0,y1,y3,y2,z0,z1,z3,z2
        
    if t>=Tm:
        c=cos(d*cos(pi*t/Tm))
        b=(0.5-0.5*cos(2*pi*(t-Tm)/Tm))
        x0=0
        y0=(-H0/40*b)/c
        z0=a+zz*cos(pi*t/Tm)
        x1=0
        y1=(H0*b)/c
        z1=a+zz*cos(pi*t/Tm)
        x2=0
        y2=y1
        z2=a+zz*cos(pi*t/Tm+pi)
        x3=0
        y3=y0
        z3=a+zz*cos(pi*t/Tm+pi)
        return x0,x1,x3,x2,y0,y1,y3,y2,z0,z1,z3,z2





